/**************************************************************************************************************
 **author:xiezh
 **date:2020-6-4
 **udp通信发送端。从topic中获取数据，在通过udp协议发送出去
 **端口号6666
 **************************************************************************************************************/
#include <sys/types.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <stdio.h>
#include <errno.h>
#include <ros/ros.h>
#include "std_msgs/String.h"
#include <sstream>

#define IP "192.168.91.130"
#define PORT 7778

static std_msgs::String g_msg;
void callbackUDP(const std_msgs::String::ConstPtr & msg)
{
    g_msg = *msg;
}


int main(int argc, char **argv)
{
	ros::init(argc, argv, "ctrl_client");
	ros::NodeHandle nh,nh_para("~");
std::string topic = "/ctrl/client";
    nh_para.param<std::string>("topic",topic,topic);
    ros::Subscriber sub = nh.subscribe(topic,1,callbackUDP);
    ROS_INFO("#From udp client#sub_tpk =%s",topic.c_str());


    std::string send_ip= "192.168.91.130";
    nh_para.param("send_ip",send_ip,send_ip);
    ROS_INFO("#From udp client#send_ip =%s",send_ip.c_str());

    int send_port;
    nh_para.param("send_port",send_port,PORT);
    ROS_INFO("#From udp client#send_port =%d",send_port);

    int buf_size;
    nh_para.param("buf_size",buf_size,100);
    ROS_INFO("#From udp client#buf_size =%d",buf_size);

    char ip[20]={0};
    int ip_len = send_ip.size();
    for(int i=0;i<ip_len;++i)
    {
        ip[i]=send_ip[i];
    }



    int cnt = -1;
    int sfd = -1;
    int len = -1;
    struct sockaddr_in ser_addr;
    char*buf = new char[buf_size];
    memset(buf,0,buf_size);
//    char buf[128] = {0};
//    char client_buf[128] = {0};

    //socket
    sfd = socket(AF_INET, SOCK_DGRAM, 0);
    if(0 > sfd)
    {
        perror("socket error:");
        return -1;
    }

    //init
    len = sizeof(struct sockaddr);
    ser_addr.sin_family = AF_INET;
    ser_addr.sin_port = htons(send_port);
    ser_addr.sin_addr.s_addr = inet_addr(ip);

    ros::Rate loop_rate(10);
    while(ros::ok())
    {
        ros::spinOnce();
        int msg_size = g_msg.data.size();
        if(msg_size<0)
        {
            loop_rate.sleep();
            continue;
        }

        for(int i=0;i<msg_size;++i)
        {
            buf[i] = g_msg.data[i];
            if(i==(buf_size -1))
            {
                msg_size = buf_size -1;
                break;
            }
        }

        cnt = sendto(sfd, buf, msg_size, 0, (struct sockaddr *)&ser_addr, len);
        if(0 > cnt)
        {
            perror("sendto error:");
           return -1;
        }
        loop_rate.sleep();
        //printf("cnt = %d\n",cnt);


        //printf("dend data = %s\n",client_buf);
    }
    delete(buf);
    close(sfd);
	return 0;
_out:
	return -1;
}
